#include <stdio.h>
#include <emmintrin.h>
#define ROWS_PROCESSED 4
#define COLS_PROCESSED 5

void sgemm( int m, int n, float *A, float *C ) {
	__m128 pivot_vector[5], column_vector, product_vector, memory_vector;
	int NUM_EXTRA_ROWS = m % ROWS_PROCESSED, NUM_EXTRA_COLS = n % COLS_PROCESSED;
	float *col_offset_A, *edge_location_A, *memory_location, pivot;

	for (int j = 0; j < m; j++) {
		for (int k = 0; k < n/COLS_PROCESSED * COLS_PROCESSED; k+= COLS_PROCESSED) {
			col_offset_A = A + k*m;

			*pivot_vector = _mm_load1_ps(col_offset_A + j);
			*(pivot_vector+1) = _mm_load1_ps(col_offset_A + j + m);
			*(pivot_vector+2) = _mm_load1_ps(col_offset_A + j + 2*m);
			*(pivot_vector+3) = _mm_load1_ps(col_offset_A + j + 3*m);
			*(pivot_vector+4) = _mm_load1_ps(col_offset_A + j + 4*m);

			for (int i = 0; i < m/ROWS_PROCESSED*ROWS_PROCESSED; i+=ROWS_PROCESSED) {
				memory_location = C + i + j*m;

				memory_vector = _mm_loadu_ps(memory_location);

				column_vector = _mm_loadu_ps(col_offset_A + i);
				product_vector = _mm_mul_ps(column_vector, *pivot_vector);
				memory_vector = _mm_add_ps(memory_vector, product_vector);

				column_vector = _mm_loadu_ps(col_offset_A + i + m);
				product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 1));
				memory_vector = _mm_add_ps(memory_vector, product_vector);

				column_vector = _mm_loadu_ps(col_offset_A + i + 2*m);
				product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 2));
				memory_vector = _mm_add_ps(memory_vector, product_vector);

				column_vector = _mm_loadu_ps(col_offset_A + i + 3*m);
				product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 3));
				memory_vector = _mm_add_ps(memory_vector, product_vector);

				column_vector = _mm_loadu_ps(col_offset_A + i + 4*m);
				product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 4));
				memory_vector = _mm_add_ps(memory_vector, product_vector);

				_mm_storeu_ps(memory_location, memory_vector);
			}

			// Handle special case when reaching bottom of matrix.
			switch (NUM_EXTRA_ROWS) {
			case 0: // Do nothing.  No handling, MUST BE FIRST.
				break;
			case 1:
				memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

				pivot = *(col_offset_A + j);
				edge_location_A = A + (m - NUM_EXTRA_ROWS) + k*m;
				*memory_location += pivot * *edge_location_A;

				pivot = *(col_offset_A + j + m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;

				pivot = *(col_offset_A + j + 2*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;

				pivot = *(col_offset_A + j + 3*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;

				pivot = *(col_offset_A + j + 4*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				break;
			case 2:
				memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

				pivot = *(col_offset_A + j);
				edge_location_A = A + (m - NUM_EXTRA_ROWS) + k*m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);

				pivot = *(col_offset_A + j + m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);

				pivot = *(col_offset_A + j + 2*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);

				pivot = *(col_offset_A + j + 3*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);

				pivot = *(col_offset_A + j + 4*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);
				break;
			case 3:
				memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

				pivot = *(col_offset_A + j);
				edge_location_A = A + (m - NUM_EXTRA_ROWS) + k*m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);
				*(memory_location + 2) += pivot * *(edge_location_A + 2);

				pivot = *(col_offset_A + j + m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);
				*(memory_location + 2) += pivot * *(edge_location_A + 2);

				pivot = *(col_offset_A + j + 2*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);
				*(memory_location + 2) += pivot * *(edge_location_A + 2);

				pivot = *(col_offset_A + j + 3*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);
				*(memory_location + 2) += pivot * *(edge_location_A + 2);

				pivot = *(col_offset_A + j + 4*m);
				edge_location_A += m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);
				*(memory_location + 2) += pivot * *(edge_location_A + 2);
				break;
			default:
				printf("Error with edge case handling: rows.");
				break;
			}
		}

		// Handle special cases for extra columns.
		int l;
		switch (NUM_EXTRA_COLS) {
		case 0:	// DO NOTHING for 60x60 case.
			break;
		case 1:
			l = n / COLS_PROCESSED * COLS_PROCESSED;
			col_offset_A = A + l*m;

			*pivot_vector = _mm_load1_ps(col_offset_A + j);

			for (int i = 0; i < m/ROWS_PROCESSED*ROWS_PROCESSED; i+=ROWS_PROCESSED) {
				memory_location = C + i + j*m;

				memory_vector = _mm_loadu_ps(memory_location);

				column_vector = _mm_loadu_ps(col_offset_A + i);
				product_vector = _mm_mul_ps(column_vector, *pivot_vector);
				memory_vector = _mm_add_ps(memory_vector, product_vector);

				_mm_storeu_ps(memory_location, memory_vector);
			}

			// Handle special case when reaching bottom of matrix.
			switch (NUM_EXTRA_ROWS) {
			case 0: // Do nothing.  No handling, MUST BE FIRST.
				break;
			case 1:
				memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

				pivot = *(col_offset_A + j);
				edge_location_A = A + (m - NUM_EXTRA_ROWS) +  l*m;
				*memory_location += pivot * *edge_location_A;

				break;
			case 2:
				memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

				pivot = *(col_offset_A + j);
				edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);

				break;
			case 3:
				memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

				pivot = *(col_offset_A + j);
				edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
				*memory_location += pivot * *edge_location_A;
				*(memory_location + 1) += pivot * *(edge_location_A + 1);
				*(memory_location + 2) += pivot * *(edge_location_A + 2);

				break;
			default:
				printf("Error with edge case handling: rows.");
				break;
			}
			break;

			case 2:
				l = n / COLS_PROCESSED * COLS_PROCESSED;
				col_offset_A = A + l*m;

				*pivot_vector = _mm_load1_ps(col_offset_A + j);
				*(pivot_vector + 1) = _mm_load1_ps(col_offset_A + j + m);

				for (int i = 0; i < m/ROWS_PROCESSED*ROWS_PROCESSED; i+=ROWS_PROCESSED) {
					memory_location = C + i + j*m;

					memory_vector = _mm_loadu_ps(memory_location);

					column_vector = _mm_loadu_ps(col_offset_A + i);
					product_vector = _mm_mul_ps(column_vector, *pivot_vector);
					memory_vector = _mm_add_ps(memory_vector, product_vector);

					column_vector = _mm_loadu_ps(col_offset_A + i + m);
					product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 1));
					memory_vector = _mm_add_ps(memory_vector, product_vector);

					_mm_storeu_ps(memory_location, memory_vector);
				}

				// Handle special case when reaching bottom of matrix.
				switch (NUM_EXTRA_ROWS) {
				case 0: // Do nothing.  No handling, MUST BE FIRST.
					break;
				case 1:
					memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

					pivot = *(col_offset_A + j);
					edge_location_A = A + (m - NUM_EXTRA_ROWS) +  l*m;
					*memory_location += pivot * *edge_location_A;

					pivot = *(col_offset_A + j + m);
					edge_location_A += m;
					*memory_location += pivot * *edge_location_A;

					break;
				case 2:
					memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

					pivot = *(col_offset_A + j);
					edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
					*memory_location += pivot * *edge_location_A;
					*(memory_location + 1) += pivot * *(edge_location_A + 1);

					pivot = *(col_offset_A + j + m);
					edge_location_A += m;
					*memory_location += pivot * *edge_location_A;
					*(memory_location + 1) += pivot * *(edge_location_A + 1);

					break;
				case 3:
					memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

					pivot = *(col_offset_A + j);
					edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
					*memory_location += pivot * *edge_location_A;
					*(memory_location + 1) += pivot * *(edge_location_A + 1);
					*(memory_location + 2) += pivot * *(edge_location_A + 2);

					pivot = *(col_offset_A + j + m);
					edge_location_A += m;
					*memory_location += pivot * *edge_location_A;
					*(memory_location + 1) += pivot * *(edge_location_A + 1);
					*(memory_location + 2) += pivot * *(edge_location_A + 2);

					break;
				default:
					printf("Error with edge case handling: rows.");
					break;
				}
				break;

				case 3:
					l = n / COLS_PROCESSED * COLS_PROCESSED;
					col_offset_A = A + l*m;

					*pivot_vector = _mm_load1_ps(col_offset_A + j);
					*(pivot_vector + 1) = _mm_load1_ps(col_offset_A + j + m);
					*(pivot_vector + 2) = _mm_load1_ps(col_offset_A + j + 2*m);

					for (int i = 0; i < m/ROWS_PROCESSED*ROWS_PROCESSED; i+=ROWS_PROCESSED) {
						memory_location = C + i + j*m;

						memory_vector = _mm_loadu_ps(memory_location);

						column_vector = _mm_loadu_ps(col_offset_A + i);
						product_vector = _mm_mul_ps(column_vector, *pivot_vector);
						memory_vector = _mm_add_ps(memory_vector, product_vector);

						column_vector = _mm_loadu_ps(col_offset_A + i + m);
						product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 1));
						memory_vector = _mm_add_ps(memory_vector, product_vector);

						column_vector = _mm_loadu_ps(col_offset_A + i + 2*m);
						product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 2));
						memory_vector = _mm_add_ps(memory_vector, product_vector);

						_mm_storeu_ps(memory_location, memory_vector);
					}

					// Handle special case when reaching bottom of matrix.
					switch (NUM_EXTRA_ROWS) {
					case 0: // Do nothing.  No handling, MUST BE FIRST.
						break;
					case 1:
						memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

						pivot = *(col_offset_A + j);
						edge_location_A = A + (m - NUM_EXTRA_ROWS) +  l*m;
						*memory_location += pivot * *edge_location_A;

						pivot = *(col_offset_A + j + m);
						edge_location_A += m;
						*memory_location += pivot * *edge_location_A;

						pivot = *(col_offset_A + j + 2*m);
						edge_location_A += m;
						*memory_location += pivot * *edge_location_A;

						break;
					case 2:
						memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

						pivot = *(col_offset_A + j);
						edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
						*memory_location += pivot * *edge_location_A;
						*(memory_location + 1) += pivot * *(edge_location_A + 1);

						pivot = *(col_offset_A + j + m);
						edge_location_A += m;
						*memory_location += pivot * *edge_location_A;
						*(memory_location + 1) += pivot * *(edge_location_A + 1);

						pivot = *(col_offset_A + j + 2*m);
						edge_location_A += m;
						*memory_location += pivot * *edge_location_A;
						*(memory_location + 1) += pivot * *(edge_location_A + 1);

						break;
					case 3:
						memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

						pivot = *(col_offset_A + j);
						edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
						*memory_location += pivot * *edge_location_A;
						*(memory_location + 1) += pivot * *(edge_location_A + 1);
						*(memory_location + 2) += pivot * *(edge_location_A + 2);

						pivot = *(col_offset_A + j + m);
						edge_location_A += m;
						*memory_location += pivot * *edge_location_A;
						*(memory_location + 1) += pivot * *(edge_location_A + 1);
						*(memory_location + 2) += pivot * *(edge_location_A + 2);

						pivot = *(col_offset_A + j + 2*m);
						edge_location_A += m;
						*memory_location += pivot * *edge_location_A;
						*(memory_location + 1) += pivot * *(edge_location_A + 1);
						*(memory_location + 2) += pivot * *(edge_location_A + 2);

						break;
					default:
						printf("Error with edge case handling: rows.");
						break;
					}
					break;

					case 4:
						l = n / COLS_PROCESSED * COLS_PROCESSED;
						col_offset_A = A + l*m;

						*pivot_vector = _mm_load1_ps(col_offset_A + j);
						*(pivot_vector + 1) = _mm_load1_ps(col_offset_A + j + m);
						*(pivot_vector + 2) = _mm_load1_ps(col_offset_A + j + 2*m);
						*(pivot_vector + 3) = _mm_load1_ps(col_offset_A + j + 3*m);

						for (int i = 0; i < m/ROWS_PROCESSED*ROWS_PROCESSED; i+=ROWS_PROCESSED) {
							memory_location = C + i + j*m;

							memory_vector = _mm_loadu_ps(memory_location);

							column_vector = _mm_loadu_ps(col_offset_A + i);
							product_vector = _mm_mul_ps(column_vector, *pivot_vector);
							memory_vector = _mm_add_ps(memory_vector, product_vector);

							column_vector = _mm_loadu_ps(col_offset_A + i + m);
							product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 1));
							memory_vector = _mm_add_ps(memory_vector, product_vector);

							column_vector = _mm_loadu_ps(col_offset_A + i + 2*m);
							product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 2));
							memory_vector = _mm_add_ps(memory_vector, product_vector);

							column_vector = _mm_loadu_ps(col_offset_A + i + 3*m);
							product_vector = _mm_mul_ps(column_vector, *(pivot_vector + 3));
							memory_vector = _mm_add_ps(memory_vector, product_vector);

							_mm_storeu_ps(memory_location, memory_vector);
						}

						// Handle special case when reaching bottom of matrix.
						switch (NUM_EXTRA_ROWS) {
						case 0: // Do nothing.  No handling, MUST BE FIRST.
							break;
						case 1:
							memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

							pivot = *(col_offset_A + j);
							edge_location_A = A + (m - NUM_EXTRA_ROWS) +  l*m;
							*memory_location += pivot * *edge_location_A;

							pivot = *(col_offset_A + j + m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;

							pivot = *(col_offset_A + j + 2*m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;

							pivot = *(col_offset_A + j + 3*m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;

							break;
						case 2:
							memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

							pivot = *(col_offset_A + j);
							edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);

							pivot = *(col_offset_A + j + m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);

							pivot = *(col_offset_A + j + 2*m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);

							pivot = *(col_offset_A + j + 3*m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);

							break;
						case 3:
							memory_location = C + (m - NUM_EXTRA_ROWS) + j*m;

							pivot = *(col_offset_A + j);
							edge_location_A = A + (m - NUM_EXTRA_ROWS) + l*m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);
							*(memory_location + 2) += pivot * *(edge_location_A + 2);

							pivot = *(col_offset_A + j + m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);
							*(memory_location + 2) += pivot * *(edge_location_A + 2);

							pivot = *(col_offset_A + j + 2*m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);
							*(memory_location + 2) += pivot * *(edge_location_A + 2);

							pivot = *(col_offset_A + j + 3*m);
							edge_location_A += m;
							*memory_location += pivot * *edge_location_A;
							*(memory_location + 1) += pivot * *(edge_location_A + 1);
							*(memory_location + 2) += pivot * *(edge_location_A + 2);

							break;
						default:
							printf("Error with edge case handling: rows.");
							break;
						}
						break;

						default:
							printf("Error with edge  case handling: cols.");
							break;
		}
	}
}
